package Detour

import (
	"math"
)

func ComputeTileHash(x, y int, mask int) int {
	var h1, h2 uint
	h1 = 0x8da6b343
	h2 = 0xd8163841
	n := (h1*uint(x) + h2*uint(y))
	return int(n) & mask
}

type DTNavMesh struct {
	M_Params                  *DTNavMeshParams
	M_Orig                    []float64
	M_TileWidth, M_TileHeight float64
	M_MaxTiles                int
	M_TileLutSize             int
	M_TileLutMask             int

	//DTMeshTile
	M_PosLookup []*DTMeshTile
	M_NextFree  *DTMeshTile
	M_Tiles     []*DTMeshTile

	M_SaltBits uint
	M_TileBits uint
	M_PolyBits uint
}

func NewDTNavMesh() *DTNavMesh {
	mesh := &DTNavMesh{}
	mesh.M_Orig = make([]float64, 3)
	mesh.M_Params = NewDTNavMeshParams()
	return mesh
}

func (this *DTNavMesh) Init(navMeshParams *DTNavMeshParams) DTStatus {
	this.M_Params = navMeshParams.Clone()
	DTVcopy(this.M_Orig, navMeshParams.Orig)
	this.M_TileWidth = navMeshParams.TileWidth
	this.M_TileHeight = navMeshParams.TileHeight
	//init tiles
	this.M_MaxTiles = navMeshParams.MaxTiles
	this.M_TileLutSize = int(DTNextPow2(uint(navMeshParams.MaxTiles / 4)))
	if this.M_TileLutSize == 0 {
		this.M_TileLutSize = 1
	}
	this.M_TileLutMask = this.M_TileLutSize - 1
	this.M_Tiles = NewDTMeshTileNum(this.M_MaxTiles)
	if this.M_Tiles == nil {
		return DT_FAILURE | DT_OUT_OF_MEMORY
	}
	this.M_PosLookup = NewDTMeshTileNum(this.M_TileLutSize)
	if this.M_PosLookup == nil {
		return DT_FAILURE | DT_OUT_OF_MEMORY
	}
	this.M_NextFree = nil
	for i := this.M_MaxTiles - 1; i >= 0; i-- {
		this.M_Tiles[i].Salt = 1
		this.M_Tiles[i].Next = this.M_NextFree
		this.M_NextFree = this.M_Tiles[i]
	}
	this.M_TileBits = uint(dtIlog2(DTNextPow2(uint(navMeshParams.MaxTiles))))
	this.M_PolyBits = uint(dtIlog2(DTNextPow2(uint(navMeshParams.MaxPolys))))
	num1, num2 := uint(31), 32-this.M_TileBits-this.M_PolyBits
	if num1 < num2 {
		this.M_SaltBits = num1
	} else {
		this.M_SaltBits = num2
	}

	return DT_SUCCESS
}

func (this *DTNavMesh) CalcTileLoc(pos []float64, tx, ty *int) {
	*tx = int(math.Floor((pos[0] - this.M_Orig[0]) / this.M_TileWidth))
	*ty = int(math.Floor((pos[2] - this.M_Orig[2]) / this.M_TileHeight))
}

func (this *DTNavMesh) InitByRawData(rawTile *DTRawTileData, Flags int) DTStatus {
	header := rawTile.Header
	if header.Magic != DT_NAVMESH_MAGIC {
		return DT_FAILURE | DT_WRONG_MAGIC
	}
	if header.Version != DT_NAVMESH_VERSION {
		return DT_FAILURE | DT_WRONG_VERSION
	}
	navMeshParams := NewDTNavMeshParams()
	DTVcopy(navMeshParams.Orig, header.Bmin)
	navMeshParams.TileWidth = header.Bmax[0] - header.Bmin[0]
	navMeshParams.TileHeight = header.Bmax[2] - header.Bmin[2]
	navMeshParams.MaxTiles = 1
	navMeshParams.MaxPolys = header.PolyCount
	status := this.Init(navMeshParams)
	if DTStatusFailed(status) {
		return status
	}
	var dummyResult DTTileRef
	dummyResult = 0
	return this.addTile(rawTile, Flags, 0, &dummyResult)
}

func (this *DTNavMesh) addTile(rawTileData *DTRawTileData, Flags int, lastRef DTTileRef, result *DTTileRef) DTStatus {
	header := rawTileData.Header
	// Make sure the data is in right format.
	if header.Magic != DT_NAVMESH_MAGIC {
		return DT_FAILURE | DT_WRONG_MAGIC
	}
	if header.Version != DT_NAVMESH_VERSION {
		return DT_FAILURE | DT_WRONG_VERSION
	}
	// Make sure the location is free.
	if this.GetTileAt(header.X, header.Y, header.Layer) != nil {
		return DT_FAILURE
	}
	// Allocate a tile.
	var tile *DTMeshTile
	if lastRef == 0 {
		if this.M_NextFree != nil {
			tile = this.M_NextFree
			this.M_NextFree = tile.Next
			tile.Next = nil
		}
	} else {
		tileIndex := this.DecodePolyIdTile(DTPolyRef(lastRef))
		if int(tileIndex) >= int(this.M_MaxTiles) {
			return DT_FAILURE | DT_OUT_OF_MEMORY
		}
		target := this.M_Tiles[tileIndex]
		var prev *DTMeshTile
		prev = nil
		tile = this.M_NextFree
		for tile != nil && tile != target {
			prev = tile
			tile = tile.Next
		}
		if tile != target {
			return DT_FAILURE | DT_OUT_OF_MEMORY
		}
		if prev == nil {
			this.M_NextFree = tile.Next
		} else {
			prev.Next = tile.Next
		}
		tile.Salt = this.DecodePolyIdSalt(DTPolyRef(lastRef))
	}
	if tile == nil {
		return DT_FAILURE | DT_OUT_OF_MEMORY
	}
	h := ComputeTileHash(header.X, header.Y, this.M_TileLutMask)
	tile.Next = this.M_PosLookup[h]
	this.M_PosLookup[h] = tile
	/// Patch header pointers.
	tile.Verts = rawTileData.Verts
	tile.Polys = rawTileData.Polys
	tile.Links = rawTileData.Links
	tile.DetailMeshes = rawTileData.DetailMeshes
	tile.DetailVerts = rawTileData.DetailVerts
	tile.DetailTris = rawTileData.DetailTris
	tile.BVTree = rawTileData.BVTree
	tile.OffMeshCons = rawTileData.OffMeshCons
	// Build links freelist
	tile.LinksFreeList = 0
	tile.Links[header.MaxLinkCount-1].Next = DT_NULL_LINK
	for i := 0; i < header.MaxLinkCount; i++ {
		tile.Links[i].Next = uint(i) + 1
	}
	//Init tile.
	tile.Header = header
	tile.Data = rawTileData
	tile.Flags = Flags
	this.connectIntLinks(tile)
	this.baseOffMeshLinks(tile)

	MAX_NEIS := 32
	neis := make([]DTMeshTile, MAX_NEIS)
	nneis := this.getTilesAt(header.X, header.Y, neis, MAX_NEIS)
	for j := 0; j < nneis; j++ {
		if &neis[j] != tile {
			this.connectExtLinks(tile, &neis[j], -1)
			this.connectExtLinks(&neis[j], tile, -1)
		}
	}

	for i := 0; i < 8; i++ {
		nneis := this.GetNeighbourTilesAt(header.X, header.Y, i, neis, MAX_NEIS)
		for j := 0; j < nneis; j++ {
			this.connectExtLinks(tile, &neis[j], i)
			this.connectExtLinks(&neis[j], tile, dtOppositeTile(i))
			this.connectExtOffMeshLinks(tile, &neis[j], i)
			this.connectExtOffMeshLinks(&neis[j], tile, dtOppositeTile(i))
		}
	}
	*result = this.getTileRef(tile)
	return DT_SUCCESS
}
func (this *DTNavMesh) getTileRef(tile *DTMeshTile) DTTileRef {
	if tile == nil {
		return 0
	}
	it := meshTileIndexOf(this.M_Tiles, tile)

	return DTTileRef(this.EncodePolyId(tile.Salt, uint(it), 0))
}

func (this *DTNavMesh) GetNeighbourTilesAt(x, y, side int, tiles []DTMeshTile, maxTiles int) int {
	nx, ny := x, y
	switch side {
	case 0:
		nx++
	case 1:
		nx++
		ny++
	case 2:
		ny++
	case 3:
		nx--
		ny++
	case 4:
		nx--
	case 5:
		nx--
		ny--
	case 6:
		ny--
	case 7:
		nx++
		ny--
	}
	return this.getTilesAt(nx, ny, tiles, maxTiles)
}

func (this *DTNavMesh) connectExtLinks(tile *DTMeshTile, target *DTMeshTile, side int) {
	if tile != nil {
		return
	}
	for i := 0; i < tile.Header.PolyCount; i++ {
		poly := tile.Polys[i]
		nv := int(poly.VertCount)
		for j := 0; j < nv; j++ {
			if (poly.Neis[j] & DT_EXT_LINK) == 0 {
				continue
			}
			dir := int(poly.Neis[j] & 0xff)
			if side != -1 && dir != side {
				continue
			}
			vaStart := poly.Verts[j] * 3
			vbStart := poly.Verts[(j+1)%nv] * 3
			nei := make([]DTPolyRef, 4)
			neia := make([]float64, 4*2)
			nnei := this.findConnectingPolys(tile.Verts, int(vaStart), tile.Verts, int(vbStart), target, dtOppositeTile(dir), nei, neia, 4)
			for k := 0; k < nnei; k++ {
				idx := allocLink(tile)
				if idx != DT_NULL_LINK {
					link := tile.Links[idx]
					link.polyRef = nei[k]
					link.Edge = byte(j)
					link.Side = byte(dir)
					link.Next = poly.FirstLink
					poly.FirstLink = idx
					if dir == 0 || dir == 4 {
						tmin := (neia[k*2+0] - tile.Verts[vaStart+2]) / (tile.Verts[vbStart+2] - tile.Verts[vaStart+2])
						tmax := (neia[k*2+1] - tile.Verts[vaStart+2]) / (tile.Verts[vbStart+2] - tile.Verts[vaStart+2])
						if tmin > tmax {
							DTSwapFloat(&tmin, &tmax)
						}
						link.Bmin = byte(DTClampFloat(tmin, 0.0, 1.0) * 255.0)
						link.Bmax = byte(DTClampFloat(tmax, 0.0, 1.0) * 255.0)
					} else if dir == 2 || dir == 6 {
						tmin := (neia[k*2+0] - tile.Verts[vaStart+0]) / (tile.Verts[vbStart+0] - tile.Verts[vaStart+0])
						tmax := (neia[k*2+1] - tile.Verts[vaStart+0]) / (tile.Verts[vbStart+0] - tile.Verts[vaStart+0])
						if tmin > tmax {
							DTSwapFloat(&tmin, &tmax)
						}
						link.Bmin = byte(DTClampFloat(tmin, 0.0, 1.0) * 255.0)
						link.Bmax = byte(DTClampFloat(tmax, 0.0, 1.0) * 255.0)
					}
				}
			}
		}
	}
}

func (this *DTNavMesh) findConnectingPolys(va []float64, vaStart int, vb []float64, vbStart int, tile *DTMeshTile, side int,
	con []DTPolyRef, conarea []float64, maxcon int) int {
	if tile == nil {
		return 0
	}
	amin := make([]float64, 2)
	amax := make([]float64, 2)
	calcSlabEndPoints(va, vaStart, vb, vbStart, amin, amax, side)
	aPos := getSlabCoord3(va, vaStart, side)
	bmin := make([]float64, 2)
	bmax := make([]float64, 2)
	m := uint16(DT_EXT_LINK | side)
	n := 0
	polyRefBase := this.GetPolyRefBase(tile)
	for i := 0; i < tile.Header.PolyCount; i++ {
		poly := tile.Polys[i]
		nv := int(poly.VertCount)
		for j := 0; j < nv; j++ {
			if poly.Neis[j] != m {
				continue
			}
			vcStart := poly.Verts[j] * 3
			vdStart := poly.Verts[(j+1)%nv] * 3
			bpos := getSlabCoord3(tile.Verts, int(vcStart), side)

			if math.Abs(aPos-bpos) > 0.01 {
				continue
			}
			calcSlabEndPoints(tile.Verts, int(vcStart), tile.Verts, int(vdStart), bmin, bmax, side)
			if overlapSlabs(amin, amax, bmin, bmax, 0.01, tile.Header.WalkableClimb) {
				continue
			}

			if n < maxcon {
				conarea[n*2+0] = math.Max(amin[0], bmin[0])
				conarea[n*2+1] = math.Max(amax[0], bmax[0])
				con[n] = polyRefBase | DTPolyRef(i)
				n++
			}
			break
		}
	}

	return n
}

func overlapSlabs(amin, amax, bmin, bmax []float64, px, py float64) bool {
	minx := math.Max(amin[0]+px, bmin[0]+px)
	maxx := math.Min(amax[0]-px, bmax[0]-px)
	if minx > maxx {
		return false
	}
	ad := (amax[1] - amin[1]) / (amax[0] - amin[0])
	ak := amin[1] - ad*amin[0]
	bd := (bmax[1] - bmin[1]) / (bmax[0] - bmin[0])
	bk := bmin[1] - bd*bmin[0]
	aminy := ad*minx + ak
	amaxy := ad*maxx + ak
	bminy := bd*maxx + bk
	bmaxy := bd*maxx + bk
	dmin := bminy - aminy
	dmax := bmaxy - amaxy
	if dmin*dmax < 0 {
		return true
	}

	thr := dtSqr(py * 2)
	if dmin*dmin <= thr || dmax*dmax <= thr {
		return true
	}
	return false
}

func calcSlabEndPoints(va []float64, vaStart int, vb []float64, vbStart int, bmin []float64, bmax []float64, side int) {
	if side == 0 || side == 4 {
		if va[vaStart+2] < vb[vbStart+2] {
			bmin[0] = va[vaStart+2]
			bmin[1] = va[vaStart+1]
			bmax[0] = vb[vbStart+2]
			bmax[1] = vb[vbStart+1]
		} else {
			bmin[0] = vb[vbStart+2]
			bmin[1] = vb[vbStart+1]
			bmax[0] = va[vaStart+2]
			bmax[1] = va[vaStart+1]
		}
	} else if side == 2 || side == 6 {
		if va[vaStart+0] < vb[0] {
			bmin[0] = va[vaStart+0]
			bmin[1] = va[vaStart+1]
			bmax[0] = vb[vbStart+0]
			bmax[1] = vb[vbStart+1]
		} else {
			bmin[0] = vb[vbStart+0]
			bmin[1] = vb[vbStart+1]
			bmax[0] = va[vaStart+0]
			bmax[1] = va[vaStart+1]
		}
	}
}
func getSlabCoord(va []float64, side int) float64 {
	if side == 0 || side == 4 {
		return va[0]
	} else if side == 2 || side == 6 {
		return va[2]
	}
	return 0
}

func getSlabCoord3(va []float64, vaStart int, side int) float64 {
	if side == 0 || side == 4 {
		return va[vaStart+0]
	} else if side == 2 || side == 6 {
		return va[vaStart+2]
	}
	return 0
}

func (this *DTNavMesh) baseOffMeshLinks(tile *DTMeshTile) {
	if tile == nil {
		return
	}
	polyRefBase := this.GetPolyRefBase(tile)
	for i := 0; i < tile.Header.OffMeshConCount; i++ {
		con := tile.OffMeshCons[i]
		poly := tile.Polys[con.Poly]
		ext := []float64{con.Rad, tile.Header.WalkableClimb, con.Rad}
		nearestPt := make([]float64, 3)
		polyRef := this.findNearestPolyInTile(tile, con.Pos, 0, ext, nearestPt)
		if polyRef == 0 {
			continue
		}
		if dtSqr(nearestPt[0]-con.Pos[0])+dtSqr(nearestPt[2]-con.Pos[2]) > dtSqr(con.Rad) {
			continue
		}
		vIndex := poly.Verts[0] * 3
		dtVcopy4(tile.Verts, int(vIndex), nearestPt, 0)
		idx := allocLink(tile)
		if idx != DT_NULL_LINK {
			link := tile.Links[idx]
			link.polyRef = polyRef
			link.Edge = 0
			link.Side = 0xff
			link.Bmin, link.Bmax = 0, 0
			link.Next = poly.FirstLink
			poly.FirstLink = idx
		}
		tidx := allocLink(tile)
		if tidx != DT_NULL_LINK {
			landPolyIdx := this.DecodePolyIdPoly(polyRef)
			lanPoly := tile.Polys[landPolyIdx]
			link := tile.Links[tidx]
			link.polyRef = polyRefBase | DTPolyRef(con.Poly)
			link.Edge = 0xff
			link.Side = 0xff
			link.Bmin, link.Bmax = 0, 0
			link.Next = lanPoly.FirstLink
			lanPoly.FirstLink = tidx
		}
	}
}

func (this *DTNavMesh) findNearestPolyInTile(tile *DTMeshTile, center []float64, centerStart int,
	extents []float64, nearestPt []float64) DTPolyRef {
	bmin := make([]float64, 3)
	bmax := make([]float64, 3)
	dtVsub6(bmin, 0, center, centerStart, extents, 0)
	DtVadd6(bmax, 0, center, centerStart, extents, 0)

	polys := make([]DTPolyRef, 128)
	polyCount := this.queryPolygonsInTile(tile, bmin, bmax, polys, 128)
	var nearest DTPolyRef = 0
	nearestDistanceSqr := 99999999.0
	for i := 0; i < polyCount; i++ {
		polyRef := polys[i]
		closestPtPoly := make([]float64, 3)
		diff := make([]float64, 3)
		posOverPoly := false
		d := 0.0
		this.closestPointOnPoly(polyRef, center, centerStart, closestPtPoly, &posOverPoly)
		dtVsub6(diff, 0, center, centerStart, closestPtPoly, 0)
		if posOverPoly {
			d = math.Abs(diff[1]) - tile.Header.WalkableClimb
			if d > 0 {
				d = d * d
			} else {
				d = 0
			}
		} else {
			d = dtVlenSqr(diff)
		}
		if d < nearestDistanceSqr {
			DTVcopy(nearestPt, closestPtPoly)
			nearestDistanceSqr = d
			nearest = polyRef
		}
	}
	return nearest
}

func (this *DTNavMesh) connectExtOffMeshLinks(tile *DTMeshTile, target *DTMeshTile, side int) {
	if tile == nil {
		return
	}
	var oppositeSide byte
	if side == -1 {
		oppositeSide = byte(0xff)
	} else {
		oppositeSide = byte(dtOppositeTile(side))
	}
	for i := 0; i < target.Header.OffMeshConCount; i++ {
		targetCon := target.OffMeshCons[i]
		if targetCon.Side != oppositeSide {
			continue
		}
		targetPoly := target.Polys[targetCon.Poly]
		if targetPoly.FirstLink == DT_NULL_LINK {
			continue
		}
		ext := []float64{targetCon.Rad, target.Header.WalkableClimb, targetCon.Rad}
		pIndex := 3
		nearestPt := make([]float64, 3)
		polyRef := this.findNearestPolyInTile(tile, targetCon.Pos, pIndex, ext, nearestPt)
		if polyRef == 0 {
			continue
		}
		if dtSqr(nearestPt[0]-targetCon.Pos[pIndex])+dtSqr(nearestPt[2]-targetCon.Pos[pIndex+2]) > dtSqr(targetCon.Rad) {
			continue
		}
		vIndex := targetPoly.Verts[1] * 3
		dtVcopy4(target.Verts, int(vIndex), nearestPt, 0)
		idx := allocLink(target)
		if idx != DT_NULL_LINK {
			link := target.Links[idx]
			link.polyRef = polyRef
			link.Edge = byte(1)
			link.Side = oppositeSide
			link.Bmin, link.Bmax = 0, 0
			link.Next = targetPoly.FirstLink
			targetPoly.FirstLink = idx
		}
		if (targetCon.Flags & DT_OFFMESH_CON_BIDIR) != 0 {
			tidx := allocLink(tile)
			if tidx != DT_NULL_LINK {
				landPolyIdx := uint16(this.DecodePolyIdPoly(polyRef))
				landPoly := tile.Polys[landPolyIdx]
				link := tile.Links[tidx]
				link.polyRef = this.GetPolyRefBase(target) | DTPolyRef(targetCon.Poly)
				link.Edge = 0xff
				if side == -1 {
					link.Side = 0xff
				}
				link.Bmin, link.Bmax = 0, 0
				link.Next = landPoly.FirstLink
				landPoly.FirstLink = tidx
			}
		}
	}
}

func (this *DTNavMesh) closestPointOnPoly(polyRef DTPolyRef, pos []float64, posStart int, closest []float64,
	posOverPoly *bool) {
	var tile *DTMeshTile
	var poly *DTPoly
	var ip uint
	this.getTileAndPolyByRefUnsafe(polyRef, tile, poly, &ip)
	if poly.getType() == DT_POLYTYPE_OFFMESH_CONNECTION {
		v0Start := poly.Verts[0] * 3
		v1Start := poly.Verts[1] * 3
		d0 := DTVdist4(pos, posStart, tile.Verts, int(v0Start))
		d1 := DTVdist4(pos, posStart, tile.Verts, int(v1Start))
		u := d0 / (d0 + d1)
		DTVlerp7(closest, 0, tile.Verts, int(v0Start), tile.Verts, int(v1Start), u)
		*posOverPoly = false
		return
	}
	pd := tile.DetailMeshes[ip]
	verts := make([]float64, DT_VERTS_PER_POLYGON*3)
	edged := make([]float64, DT_VERTS_PER_POLYGON)
	edget := make([]float64, DT_VERTS_PER_POLYGON)
	nv := int(poly.VertCount)
	for i := 0; i < nv; i++ {
		dtVcopy4(verts, i*3, tile.Verts, int(poly.Verts[i])*3)
	}
	dtVcopy4(closest, 0, pos, posStart)
	if !dtDistancePtPolyEdgesSqr(pos, posStart, verts, nv, edged, edget) {
		dmin := 9999999.0
		imin := -1
		for i := 0; i < nv; i++ {
			if edged[i] < dmin {
				dmin = edged[i]
				imin = i
			}
		}
		vaStart := imin * 3
		vbStart := ((imin + 1) % nv) * 3
		DTVlerp7(closest, 0, verts, vaStart, verts, vbStart, edget[imin])
		*posOverPoly = false
	} else {
		*posOverPoly = true
	}
	// Find height at the location.
	for j := 0; j < int(pd.TriCount); j++ {
		tIndex := (int(pd.TriBase) + j) * 4
		vIndices := make([]int, 3)
		vArrays := make([][]float64, 3)
		for k := 0; k < 3; k++ {
			if tile.DetailTris[tIndex+k] < poly.VertCount {
				vIndices[k] = int(poly.Verts[int(tile.DetailTris[tIndex])+k] * 3)
				vArrays[k] = tile.Verts
			} else {
				vIndices[k] = (int(pd.VertBase) + (int(tile.DetailTris[tIndex+k]) - int(poly.VertCount))) * 3
				vArrays[k] = tile.DetailVerts
			}
		}

		h := 0.0
		if dtClosestHeightPointTriangle(pos, posStart, vArrays[0], vIndices[0], vArrays[1], vIndices[1], vArrays[2], vIndices[2], &h) {
			closest[1] = h
			break
		}
	}
}

func (this *DTNavMesh) IsValidPolyRef(polyRef DTPolyRef) bool {
	if polyRef == 0 {
		return false
	}

	var salt, it, ip uint
	this.DecodePolyId(polyRef, &salt, &it, &ip)
	if it >= uint(this.M_MaxTiles) {
		return false
	}
	if this.M_Tiles[it].Salt != salt || this.M_Tiles[it].Header == nil {
		return false
	}
	if ip >= uint(this.M_Tiles[it].Header.PolyCount) {
		return false
	}
	return true
}

func (this *DTNavMesh) getTileAndPolyByRefUnsafe(polyRef DTPolyRef, tile *DTMeshTile,
	poly *DTPoly, ip *uint) {
	var salt, it uint
	this.DecodePolyId(polyRef, &salt, &it, ip)
	tile = this.M_Tiles[it]
	poly = &this.M_Tiles[it].Polys[*ip]
}

func (this *DTNavMesh) getTileAndPolyByRefUnsafe3(polyRef DTPolyRef) (tile *DTMeshTile, poly *DTPoly) {
	var salt, it, ip uint
	this.DecodePolyId(polyRef, &salt, &it, &ip)
	tile = this.M_Tiles[it]
	poly = &this.M_Tiles[it].Polys[ip]

	return tile, poly
}

func (this *DTNavMesh) getTileAndPolyByRef(polyRef DTPolyRef, ip *uint) (DTStatus, *DTMeshTile, *DTPoly) {
	if polyRef == 0 {
		return DT_FAILURE, nil, nil
	}
	var salt, it uint
	this.DecodePolyId(polyRef, &salt, &it, ip)
	if it >= uint(this.M_MaxTiles) {

		return DT_FAILURE | DT_INVALID_PARAM, nil, nil
	}
	if this.M_Tiles[it].Salt != salt || this.M_Tiles[it].Header == nil {

		return DT_FAILURE | DT_INVALID_PARAM, nil, nil
	}
	if *ip >= uint(this.M_Tiles[it].Header.PolyCount) {
		return DT_FAILURE | DT_INVALID_PARAM, nil, nil
	}
	tile := this.M_Tiles[it]
	poly := &this.M_Tiles[it].Polys[*ip]
	return DT_SUCCESS, tile, poly
}

func (this *DTNavMesh) getTileAndPolyByRef3(polyRef DTPolyRef, tile **DTMeshTile, poly **DTPoly) DTStatus {
	if polyRef == 0 {
		return DT_FAILURE
	}
	var salt, it, ip uint
	this.DecodePolyId(polyRef, &salt, &it, &ip)
	if it >= uint(this.M_MaxTiles) {
		return DT_FAILURE | DT_INVALID_PARAM
	}
	if this.M_Tiles[it].Salt != salt || this.M_Tiles[it].Header == nil {
		return DT_FAILURE | DT_INVALID_PARAM
	}
	if ip >= uint(this.M_Tiles[it].Header.PolyCount) {
		return DT_FAILURE | DT_INVALID_PARAM
	}
	*tile = this.M_Tiles[it]
	*poly = &this.M_Tiles[it].Polys[ip]
	return DT_SUCCESS
}

func (this *DTNavMesh) queryPolygonsInTile(tile *DTMeshTile, qmin []float64, qmax []float64, polys []DTPolyRef, maxPolys int) int {
	if tile.BVTree != nil {
		endNodeIndex := tile.Header.BvNodeCount
		tbmin := tile.Header.Bmin
		tbmax := tile.Header.Bmax
		qfac := tile.Header.BvQuantFactor
		// Calculate quantized box
		bmin := make([]uint16, 3)
		bmax := make([]uint16, 3)
		// dtClamp query box to world box.
		minx := DTClampFloat(qmin[0], tbmin[0], tbmax[0]) - tbmin[0]
		miny := DTClampFloat(qmin[1], tbmin[1], tbmax[1]) - tbmin[1]
		minz := DTClampFloat(qmin[2], tbmin[2], tbmax[2]) - tbmin[2]
		maxx := DTClampFloat(qmax[0], tbmin[0], tbmax[0]) - tbmin[0]
		maxy := DTClampFloat(qmax[1], tbmin[1], tbmax[1]) - tbmin[1]
		maxz := DTClampFloat(qmax[2], tbmin[2], tbmax[2]) - tbmin[2]
		// Quantize
		bmin[0] = uint16(uint16(qfac*minx) & 0xfffe)
		bmin[1] = uint16(uint16(qfac*miny) & 0xfffe)
		bmin[2] = uint16(uint16(qfac*minz) & 0xfffe)
		bmax[0] = uint16(uint16(qfac*maxx+1) | 1)
		bmax[1] = uint16(uint16(qfac*maxy+1) | 1)
		bmax[2] = uint16(uint16(qfac*maxz+1) | 1)
		// Traverse tree
		polyRefBase := this.GetPolyRefBase(tile)
		n := 0
		curNode := 0
		for curNode < endNodeIndex {
			node := tile.BVTree[curNode]
			overlap := dtOverlapQuantBounds(bmin, bmax, node.Bmin, node.Bmax)
			isLeafNode := node.I >= 0
			if isLeafNode && overlap {
				if n < maxPolys {
					polys[n] = polyRefBase | DTPolyRef(node.I)
					n++
				}
			}
			if overlap || isLeafNode {
				curNode++
			} else {
				escapeIndex := -node.I
				curNode += escapeIndex
			}
		}
		return n
	} else {
		bmin := make([]float64, 3)
		bmax := make([]float64, 3)
		n := 0
		polyRefBase := this.GetPolyRefBase(tile)
		for i := 0; i < tile.Header.PolyCount; i++ {
			p := tile.Polys[i]
			if p.getType() == DT_POLYTYPE_OFFMESH_CONNECTION {
				continue
			}
			vIndex := p.Verts[0] * 3
			dtVcopy4(bmin, 0, tile.Verts, int(vIndex))
			dtVcopy4(bmax, 0, tile.Verts, int(vIndex))
			for j := 0; j < int(p.VertCount); j++ {
				vIndex := p.Verts[j] * 3
				DTVminFloat4(bmin, 0, tile.Verts, int(vIndex))
				DTVMaxFloat64(bmax, 0, tile.Verts, int(vIndex))
			}
			if dtOverlapBounds(qmin, qmax, bmin, bmax) {
				if n < maxPolys {
					polys[n] = polyRefBase | DTPolyRef(i)
					n++
				}
			}
		}
		return n
	}
}

func (this *DTNavMesh) GetTile(i int) *DTMeshTile {
	return this.M_Tiles[i]
}

func (this *DTNavMesh) GetTileAt(x, y, layer int) *DTMeshTile {
	h := ComputeTileHash(x, y, this.M_TileLutMask)
	tile := this.M_PosLookup[h]
	for tile != nil {
		if tile.Header != nil && tile.Header.X == x && tile.Header.Y == y && tile.Header.Layer == layer {
			return tile
		}
		tile = tile.Next
	}
	return nil
}

func (this *DTNavMesh) getTilesAt(x, y int, tiles []DTMeshTile, maxTiles int) int {
	n := 0
	h := ComputeTileHash(x, y, this.M_TileLutMask)
	tile := this.M_PosLookup[h]
	for tile != nil {
		if tile.Header != nil && tile.Header.X == x && tile.Header.Y == y {
			if n < maxTiles {
				tiles[n] = *tile
				n++
			}
		}
		tile = tile.Next
	}
	return n
}

func (this *DTNavMesh) DecodePolyIdPoly(polyRef DTPolyRef) uint {
	polyMask := DTPolyRef(1<<this.M_PolyBits - 1)
	return uint(polyRef & polyMask)
}

func (this *DTNavMesh) DecodePolyId(polyRef DTPolyRef, salt, it, ip *uint) {
	saltMask := DTPolyRef((1 << this.M_SaltBits) - 1)
	tileMask := DTPolyRef((1 << this.M_TileBits) - 1)
	polyMask := DTPolyRef((1 << this.M_PolyBits) - 1)

	*salt = uint((polyRef >> (this.M_PolyBits + this.M_TileBits)) & saltMask)
	*it = uint((polyRef >> this.M_PolyBits) & tileMask)
	*ip = uint(polyRef & polyMask)
}

func (this *DTNavMesh) DecodePolyIdTile(polyRef DTPolyRef) uint {
	tileMask := DTPolyRef((1 << this.M_TileBits) - 1)
	return uint((polyRef >> this.M_PolyBits) & tileMask)
}

func (this *DTNavMesh) DecodePolyIdSalt(polyRef DTPolyRef) uint {
	saltMask := DTPolyRef((1 << this.M_SaltBits) - 1)
	return uint(polyRef >> (this.M_PolyBits + this.M_TileBits) & saltMask)
}

func (this *DTNavMesh) connectIntLinks(tile *DTMeshTile) {
	if tile == nil {
		return
	}
	polyRefBase := this.GetPolyRefBase(tile)
	for i := 0; i < tile.Header.PolyCount; i++ {
		poly := &tile.Polys[i]
		poly.FirstLink = DT_NULL_LINK
		if poly.getType() == DT_POLYTYPE_OFFMESH_CONNECTION {
			continue
		}
		for j := int(poly.VertCount - 1); j >= 0; j-- {
			if poly.Neis[j] == 0 || (poly.Neis[j]&DT_EXT_LINK) != 0 {
				continue
			}
			idx := allocLink(tile)
			if idx != DT_NULL_LINK {
				link := &tile.Links[idx]
				link.polyRef = polyRefBase | DTPolyRef(poly.Neis[j]-1)
				link.Edge = byte(j)
				link.Side = 0xff
				link.Bmin, link.Bmax = 0, 0
				link.Next = poly.FirstLink
				poly.FirstLink = idx
			}
		}
	}

}

func allocLink(tile *DTMeshTile) uint {
	if tile.LinksFreeList == DT_NULL_LINK {
		return DT_NULL_LINK
	}
	link := tile.LinksFreeList
	tile.LinksFreeList = tile.Links[link].Next
	return link
}

func (this *DTNavMesh) GetPolyRefBase(tile *DTMeshTile) DTPolyRef {
	if tile == nil {
		return 0
	}
	it := meshTileIndexOf(this.M_Tiles, tile)
	return this.EncodePolyId(tile.Salt, uint(it), 0)
}

func (this *DTNavMesh) EncodePolyId(salt uint, it uint, ip uint) DTPolyRef {
	return DTPolyRef(
		DTPolyRef(salt<<(this.M_PolyBits+this.M_TileBits)) | DTPolyRef(it<<this.M_PolyBits) | DTPolyRef(ip))
}

func meshTileIndexOf(meshs []*DTMeshTile, mesh *DTMeshTile) int {
	for i := 0; i < len(meshs); i++ {
		if meshs[i].Salt == mesh.Salt {
			return i
		}
	}
	return -1
}
